package various;


import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.ColorSensor;
import lejos.nxt.comm.NXTCommConnector;
import lejos.nxt.comm.NXTConnection;
import lejos.nxt.remote.RemoteNXT;

public class ReadMatrixLinear {

	private static int angle;
	private static int cicli_int;
	private static int cicli_ex;

	public static void main(String[] args) throws Exception {

		angle = (230 / 3) * 2;
		cicli_int = 6;
		cicli_ex = 6;

		int[][] matrice = new int[cicli_ex][cicli_int];

		Colori.initColori();
		ColorSensor cs = new ColorSensor(SensorPort.S1);

		calibra(cs);
		Thread.sleep(2000);

		for (int i = 0; i < cicli_ex; i++) {
			if (i <= 2) {
				for (int j = 0; j < cicli_int; j++) {
					Motor.A.regulateSpeed(true);
					LCD.clear();
					LCD.drawString(Colori.getColore(cs.getColorNumber()), 0, 1); // legge
																					// il
																					// colore
					matrice[i][j] = cs.getColorNumber();
					Thread.sleep(1000);
					Motor.A.rotate(angle);
					Motor.A.stop();
				}
			}
			Thread.sleep(2000);
			if (i > 2) {
				for (int j = 0; j < cicli_int; j++) {
					Motor.A.regulateSpeed(true);
					Motor.A.rotate(-angle);
					Motor.A.stop();
					LCD.clear();
					LCD.drawString(Colori.getColore(cs.getColorNumber()), 0, 1);
					matrice[i][j] = cs.getColorNumber();
					Thread.sleep(1000);
				}
			}
		}

		LCD.clear();
		for (int i = 0; i < cicli_ex; i++) {
			for (int j = 0; j < cicli_int; j++) {
				String w = Colori.getColoreCh(matrice[i][j]);
				LCD.drawString(w, j, i);
			}
		}
		Thread.sleep(30000);

		RemoteNXT nxt = new RemoteNXT("nxt", new NXTCommConnector() {

			@Override
			public NXTConnection waitForConnection(int arg0, int arg1) {
				// TODO Auto-generated method stub
				return null;
			}

			@Override
			public NXTConnection connect(String arg0, int arg1) {
				// TODO Auto-generated method stub
				return null;
			}
		});

		System.out.println(nxt.getBluetoothAddress());

	}

	private static void riavvolgi(UltrasonicSensor us) {
		while (us.getDistance() >= 5) {
			Motor.A.setSpeed(300);
			Motor.A.backward();
			LCD.clear();
			LCD.drawString("cm: " + us.getDistance(), 0, 4);
		}

	}

	public static void calibra(ColorSensor cs) throws Exception {
		Motor.A.regulateSpeed(true);
		int c = 1;
		int d = 1;
		while (c == 1 && d == 1) {
			LCD.clear();
			LCD.drawString("Avvio calibrazione bianco...", 0, 2);
			c = cs.initWhiteBalance();
			LCD.drawString("Calibrazione bianco eseguita", 0, 3);
			LCD.drawInt(c, 0, 3);
			Thread.sleep(2000);

			Motor.A.rotate(angle);
			LCD.clear();
			LCD.drawString("Avvio calibrazione nero...", 0, 2);
			d = cs.initBlackLevel();
			LCD.clear();
			LCD.drawString("Calibrazione nero eseguita", 0, 3);
			LCD.drawInt(d, 0, 6);
			Thread.sleep(2000);
		}
		LCD.clear();
		Motor.A.regulateSpeed(true);
		Motor.A.rotate(-angle);
		Thread.sleep(2000);

	}

}
